/*
 * robo_control.cc
 *
 *  Created on: Apr 1, 2010
 *      Author: Aron
 *
 */

//working, 50 hz

# include "init_systems.h"
# include "rrcon.h"
# include "time_tools.h"
# include "xtask.h"
# include "robo_control.h"
# include "cmath"

using namespace std;
using namespace kthrobot;


namespace Group5 {

  Robo_Control::Robo_Control( const string &name, int motor_number) : XTask(name,99), motor(RRCon::motor(motor_number)) {
    //calc constant terms
    h_ = 1.0/hz_;
    bi_ = (K_* h_)/Ti_;
    ad_ = TD_/(TD_+N_*h_);
    bd_ = K_ * TD_ * N_;
    a0_ = h_/Tt_;
    size_k_ =  diam_*3.14/634;
    target_speed_ = 0;
    setpoint_speed_ = 0;
    actual_speed_ = 0;
    uP_mc_ = 0;
    //frequency
    SetPeriodHz(hz_);

  }

  Robo_Control::~Robo_Control(){
    Join();
  }

  void Robo_Control::TaskCode(){
    double uP = 0;
    double uI = 0;
    double uD = 0;
    double u = 0;
    double v = 0;
    double old_y = 0;
    speed_since_last_ = 0;

    actual_speed_ = 0;

    while ( !MustStop() ){

      actual_speed_ = getSpeed();
      speed_since_last_ +=actual_speed_;

      //Log::Write( "Real speed: %f", actual_speed_);
      //Log::Write( "error: %f", (setpoint_speed_-actual_speed_));

      //fix to prevent to big change in speed during 1 tick
      if (target_speed_ != setpoint_speed_){
        if(target_speed_ - setpoint_speed_ < neg_max_speed_change_){
          setpoint_speed_ += neg_max_speed_change_;
        }
        else if(target_speed_ - setpoint_speed_ > pos_max_speed_change_){
          setpoint_speed_ += pos_max_speed_change_;
        }
        else{
          setpoint_speed_ = target_speed_;
        }
      }

      //calculate everything...
      uP = K_*(setpoint_speed_-actual_speed_);
      uD = ad_*uD-bd_*(actual_speed_-old_y);
      v = u + uP + uD + uI + uP_mc_;


      //Log::Write( "speed: %f", actual_speed_);
      //Log::Write( "uP out: %f,uD out: %f,uI out: %f", uP, uD, uI);

      if(speed_since_last_ == 0 && setpoint_speed_< 0 && v > -deadband_size_){
          v = -deadband_size_;
      }
      else if (speed_since_last_ == 0 && setpoint_speed_ > 0 && v < deadband_size_){

          v = deadband_size_;
      }

      if(v > u_max_){
        u = u_max_;
      }
      else if(v < u_min_){
        u = u_min_;
      }
      else {
        u = v;
      }
      //Log::Write("u:  %f", u);

      motor.SetSpeed(-int(u));
      uI += bi_* (setpoint_speed_-actual_speed_) + a0_*(u-v);
      old_y = actual_speed_;

      WaitPeriod();
    }
  }

  void Robo_Control::SetTargetSpeed(int speed){
    if(speed == 0){
      target_speed_ = 0;
      setpoint_speed_ = 0;
      motor.SetSpeed(0);
    }
    else if(speed <= max_speed_ && speed >= -max_speed_){
      old_target_speed_ = target_speed_;
      target_speed_ = 0.1*speed;

    }
  }
  // gets the relative speed we should try to regulate towards
  //then calculates the error compared to this speed and adjusts to normal scale
  // and use this rel error to calculate the gain
  void Robo_Control::Set_uP_mc(double relSpeed){
    uP_mc_ = K_mc_ * (relSpeed *(setpoint_speed_-old_target_speed_)-actual_speed_);
  }

  //calc speed relative to target speed, 0-1
  double Robo_Control::GetRelSpeed(){
    if(setpoint_speed_-old_target_speed_ != 0){
      return std::fabs((actual_speed_)/(setpoint_speed_-old_target_speed_));
    }
    return 0;
  }

  double Robo_Control::GetLastSpeed(){
    double speed = speed_since_last_;
    speed_since_last_ = 0;
    return speed;
  }

  void Robo_Control::Stop(){
    RRCon::motor(motor_number_).SetSpeed(0);
  }

    //should give the speed, might give the wrong sign
  double Robo_Control::getSpeed(){
    EncoderData data = motor.ReadEnc();
    return size_k_*(data.prev-data.cur);
  }

};
